#ifndef CW_CONTROL_H_
#define CW_CONTROL_H_
#include "includes.h"

#define SET_CTRLFLAG(x) do{LeftMT.flag=RightMT.flag=x;}while(0)
#define GET_CTRLFLAG()  LeftMT.flag
#ifdef MCU_STM32F103RCT6
typedef enum
{
	false,
	true
}BOOLDef;
#endif
#ifdef MCU_TIVAC123
typedef enum
{
	bool_false,
	bool_true
}BOOLDef;
#endif

typedef enum
{
	MPS,
	APS,
	PPS
}SpeedUnitDef;

typedef enum
{
	MTR,
	ANG,
	PLE
}PostitionUnitDef;

typedef enum
{
	NOCTRL=0x00,
	PWMCTRL=0x01,
	SPDCTRL=0x02,
	POSCTRL=0x04,
	MODCTRL=0x08,
	TCKCTRL=0x10
}CtrlFlagDef;

typedef enum
{
	FREESTA=0x00,
	RSPDSTA=0x01,
	SPEDSTA=0x02,
	POSTSTA=0x04,
	TCKLSTA=0x10

}MotorStatusDef;

typedef enum
{
	FWD=0x55,
	REV=0xaa
}DirDef;

typedef struct
{
	float leftval;
	float rightval;
}BfloatDef;

typedef struct
{
	CtrlFlagDef flag;		//控制标志位
	DirDef DIR;				//方向
	int PWMval; 			//PWM值
	int	PVreality; 			//真实脉冲速度
	int	PVtarget; 			//目标脉冲速度
	int	PVtrack; 			//循迹修正速度
	int PVexcept;			//期望脉冲速度
	long PPreality; 		//当前脉冲路程
	long PPtarget;			//目标脉冲路程
	MotorStatusDef status;	//电机运动状态
}MotorDef;

typedef struct
{
	BOOLDef ready;
	float err;
	int mode;
	int exceptspeed;
}TrackDef;

typedef struct
{
	float wheelR;			//轮胎半径
	float wheelC;			//轮胎周长
	float wheelBase;		//驱动轴轴距
	int   lPulsePerAngle;	//左轮胎旋转1度的脉冲数
	int   rPulsePerAngle;	//右轮胎旋转1度的脉冲数
	float PulsePerAngle;	//轮胎旋转1度的脉冲数
	float PulsePerMeter;	//轮胎旋转1度的脉冲数
	BOOLDef lDirTurn;		//左轮方向反转
	BOOLDef rDirTurn;		//右轮方向反转
}PhyparamDef;

typedef struct
{
	int pwmvalmax;			//PWM可设置的最大正值
	int pwmvalprt;			//PWM可设置的保护值
}MCUparamDef;


extern MotorDef LeftMT;
extern MotorDef RightMT;
extern TrackDef Track;
extern PhyparamDef PhyParam;
extern MCUparamDef MCUParam;
extern MotorStatusDef MTSta;

extern int lsendspd;
extern int rsendspd;

//控制初始化
void control_init(void);
//--------------------------------------PWM层次------------------------------------------------------
//设置PWM
void set_pwm(int leftval,int rightval);

//--------------------------------------速度层次------------------------------------------------------
//读取速度
BfloatDef get_speed(SpeedUnitDef unit);
//设置速度
void set_speed(int leftval,int rightval,SpeedUnitDef unit);

//--------------------------------------路程层次------------------------------------------------------
//读取路程
BfloatDef get_postition(PostitionUnitDef unit);
//设置路程
void set_postition(long leftval,long rightval,PostitionUnitDef unit);
//读取剩余路程
BfloatDef get_rest_postition(PostitionUnitDef unit);

int waitposfor(double diearea,PostitionUnitDef unit,int spdclrflag);
//清空路程
void clear_postition(void);
//循迹控制
//循迹控制
void track_control(void);
//速度控制
void speed_control(void);
//位置控制
void postition_control(void);
//电机运动状态更新
void motor_status_update(void);
//--------------------------------------差速模型层次---------------------------------------------------
int modelctrl(float velocity, float omega,double distance,uint8_t choke);
//--------------------------------------功能函数------------------------------------------------------
//速度转换
float speed_turn(float oldspeed,SpeedUnitDef oldunit,SpeedUnitDef newunit);
//位移转换
double postition_turn(double olddistance,PostitionUnitDef oldunit,PostitionUnitDef newunit);

void spinto(float leftspeed,float rightspeed,SpeedUnitDef spdunit,double leftpostition,double rightpostition,PostitionUnitDef posunit);

void spinfor(float leftspeed,float rightspeed,SpeedUnitDef spdunit);
#endif
